package tc.de.krawallmann.robobridge.devices.implementation;

import tc.de.krawallmann.robobridge.communication.Command;
import tc.de.krawallmann.robobridge.communication.CommandType;
import tc.de.krawallmann.robobridge.util.MathUtil;

public class Servo {
	private RNControl rncontrol;
	

	public Servo(RNControl rncontrol) {
		super();
		this.rncontrol = rncontrol;
	}

	public static final int LEFT_TOUCH = 71; // Left servo touch (~0.7ms)
	public static final int RIGHT_TOUCH = 162; // Right servo touch (~2.3ms)
	public static final int MIDDLE_POS = 116; // Right servo touch (~2.3ms)
	private int servoPosition;
	
	public int getServoPosition() {
		return this.servoPosition;
	}

	public void setServoPosition(int servoPosition) {
		int servoPositionTrue=(int) MathUtil.interpolate(servoPosition, -90, 90, LEFT_TOUCH, RIGHT_TOUCH);
		rncontrol.getRobot().queueOutgoingCommand(
				new Command(CommandType.UPDATE_ROBOT_DEVICE, false, rncontrol.getDeviceId(),
						RNControl.SET_SERVO_COMMAND, servoPositionTrue));
	}

	public void setServoPositionInternal(int servoPosition) {
		int old = this.servoPosition;
		this.servoPosition = (int) MathUtil.interpolate(servoPosition, LEFT_TOUCH, RIGHT_TOUCH, -90, 90);
		rncontrol.firePropertyChange(RNControl.RNCONTROL_SERVO_POSITION, old, this.servoPosition);
	}
}
